/*
 * PUC-Rio - Pontificia Universidade Catolica
 *
 * Professor: Marcelo Gattass
 *
 * Arquivo: bsp.cpp
 *
 * Nome: Luiz Felipe Machado
 */

#include <math.h>
#include <algorithm>
using namespace std;

#include "GL/gl.h"
#include "GL/glut.h"

#define TOL 1.0e-5

#include "bsp.h"

/**
  * @brief Calcular intercessao
  *
  * Essa funcao verifica se o plano corta o segmento de reta. Caso corte ele retorna na
  * na variavel res a posicao que foi cortada.
  *
  * @param[in] a - ponto do segmento de reta
  * @param[in] b - ponto do segmento de reta
  * @param[in] p0 - ponto do plano
  * @param[in] nor - normal do plano
  * @param[out] pos do segmento que o plano cortou
  * @return true caso o plano tenha cortado o segmento ou
 *                      false caso o plano nao tenha cortado o segmento
  */
bool calculate_intersection(VVector a, VVector b, VVector p0, VVector nor, VVector &res);

/**
  * @brief Adicionar triangulo
  *
  * Essa funcao adiciona os vertices a, b e c a estrutura. os vertices nao sao duplicados, ou seja
  * antes de inserir eh verificado se o vertice ja existe na estrutura.
  *
  * @param[in] a - ponto do triangulo
  * @param[in] b - ponto do triangulo
  * @param[in] c - ponto do triangulo
  * @param[in] vet_tri - vetor com a lista de triangulos
  * @param[in] vet_vet - vetor com a lista de vertices
  * @param[in] vet_nor - vetor com a lista de normais
  */
void add_triangle(VVector a, VVector b, VVector c,
                  std::vector<cTriangle> &vet_tri,
                  std::vector<VVector> &vet_ver,
                  std::vector<VVector> &vet_nor);

/**
  * @brief Calcular lado
  *
  * Verifica se  o triangulo passado (3 pontos) estah na frente, atras ou devera ser cortado em
  * relacao ao plano (ponto e normal passado).
  *
  * @param[in] normal_plane - normal do plano
  * @param[in] p - ponto do plano
  * @param[in] a - ponto do triangulo
  * @param[in] b - ponto do triangulo
  * @param[in] c - ponto do triangulo
  * @return int FRONT - esta na frente, BACK - esta atras, CUT - devera cortar
  */
int calculate_side(VVector &normal_plane, VVector &p, VVector &a, VVector &b, VVector &c);

/**
  * @brief Calcular lado
  *
  * Verifica se  o ponto passado estah na frente ou atras relacao ao plano (ponto e normal passado).
  *
  * @param[in] normal_plane - normal do plano
  * @param[in] p - ponto do plano
  * @param[in] a - ponto
  * @return int FRONT - esta na frente, BACK - esta atras
  */
int calculate_side(VVector &normal_plane,  VVector &p,  VVector &a) ;

/**
  * @brief Seleciona um triangulo
  *
  * Seleciona um triangulo que possa ser utilizado como no da arvore
  *
  * @param[in] vet_tri - vetor com a lista de triangulos
  * @param[in] vet_vet - vetor com a lista de vertices
  * @param[in] vet_nor - vetor com a lista de normais
  * @return int - indice do triangulo a ser utilizado como no
  */
int select_triangle(std::vector<cTriangle> &vet_tri, std::vector<VVector> &vet_vertex, std::vector<VVector> &vet_nor);

void cut_triangle(int selected_triangle, int other_triangle,
                  std::vector<cTriangle> &vet_tri,
                  std::vector<VVector> &vet_vertex,
                  std::vector<VVector> &vet_nor,
                  std::vector<cTriangle> &vet_tri_front,
                  std::vector<VVector> &vet_vertex_front,
                  std::vector<VVector> &vet_nor_front,
                  std::vector<cTriangle> &vet_tri_back,
                  std::vector<VVector> &vet_vertex_back,
                  std::vector<VVector> &vet_nor_back);

enum { FRONT = 0, BACK, CUT};

// ------------------------------------------------------------------------------------------------
// ------------------------------------------------------------------------------------------------
void cBsp::create_bsp(std::vector<cTriangle> &vet_tri,
                      std::vector<VVector> &vet_vertex,
                      std::vector<VVector> &vet_nor)
{
  if(!m_root)
    m_root = m_insert_rec(vet_tri, vet_vertex, vet_nor);

  calculate_bbox();
}

// ------------------------------------------------------------------------------------------------
// ------------------------------------------------------------------------------------------------
cNode* cBsp::m_insert_rec(std::vector<cTriangle> &vet_tri,
                          std::vector<VVector> &vet_vertex,
                          std::vector<VVector> &vet_nor)
{
  cNode *n = NULL;

  if(vet_tri.empty())
  {
    // o no pai eh uma folha
    return n;
  }

  std::vector<cTriangle> vet_tri_front;
  std::vector<VVector> vet_vertex_front, vet_nor_front;

  std::vector<cTriangle> vet_tri_back;
  std::vector<VVector> vet_vertex_back, vet_nor_back;

  // escolher o triangulo que vai dividir a cena
  int t = select_triangle(vet_tri, vet_vertex, vet_nor);

  // separar os triangulos da direta, esquerda e os cortados
  for(int i = 0; i < (int)vet_tri.size(); i++)
  {
    if(t == i)
    {
      // nao precisa separar o triangulo escolhido

      // cria no
      n = new cNode();

      // adiciona aos vetores "globais" / "finais" o novo triangulo

      add_triangle(vet_vertex[vet_tri[t].m_v[0]], vet_vertex[vet_tri[t].m_v[1]], vet_vertex[vet_tri[t].m_v[2]], m_data_tri, m_data_vet, m_data_nor);

      n->tri = (int)m_data_tri.size() - 1;

      printf("Calculando...");
    }
    else
    {
      int a = calculate_side(vet_nor[t],  vet_vertex[vet_tri[t].m_v[0]], vet_vertex[vet_tri[i].m_v[0]], vet_vertex[vet_tri[i].m_v[1]], vet_vertex[vet_tri[i].m_v[2]]);

      if(a == CUT)
        cut_triangle(t, i, vet_tri, vet_vertex, vet_nor, vet_tri_front, vet_vertex_front, vet_nor_front, vet_tri_back, vet_vertex_back, vet_nor_back);
      else if(a == FRONT)
        add_triangle(vet_vertex[vet_tri[i].m_v[0]], vet_vertex[vet_tri[i].m_v[1]], vet_vertex[vet_tri[i].m_v[2]], vet_tri_front, vet_vertex_front, vet_nor_front);
      else if(a == BACK)
        add_triangle(vet_vertex[vet_tri[i].m_v[0]], vet_vertex[vet_tri[i].m_v[1]], vet_vertex[vet_tri[i].m_v[2]], vet_tri_back, vet_vertex_back, vet_nor_back);

    }
  }

  vet_tri.clear(); vet_vertex.clear(); vet_nor.clear();

  n->front = m_insert_rec(vet_tri_front, vet_vertex_front, vet_nor_front);
  n->back = m_insert_rec(vet_tri_back, vet_vertex_back, vet_nor_back);

  vet_tri_front.clear(); vet_vertex_front.clear(); vet_nor_front.clear();
  vet_tri_back.clear(); vet_vertex_back.clear(); vet_nor_back.clear();

  return n;
}

// ------------------------------------------------------------------------------------------------
// ------------------------------------------------------------------------------------------------
void cBsp::draw(VVector &eye)
{
  if(m_root)
    m_draw_rec(m_root, eye);
}

// ------------------------------------------------------------------------------------------------
// ------------------------------------------------------------------------------------------------
void cBsp::m_draw_node(  cNode *node)
{
    int i = node->tri;

    glBegin(GL_TRIANGLES);
    {
      glNormal3f(m_data_nor[i].x, m_data_nor[i].y, m_data_nor[i].z);

      glVertex3d(m_data_vet[m_data_tri[i].m_v[X]].x,
                 m_data_vet[m_data_tri[i].m_v[X]].y,
                 m_data_vet[m_data_tri[i].m_v[X]].z);

      glNormal3f(m_data_nor[i].x, m_data_nor[i].y, m_data_nor[i].z);

      glVertex3d(m_data_vet[m_data_tri[i].m_v[Y]].x,
                 m_data_vet[m_data_tri[i].m_v[Y]].y,
                 m_data_vet[m_data_tri[i].m_v[Y]].z);

      glNormal3f(m_data_nor[i].x, m_data_nor[i].y, m_data_nor[i].z);

      glVertex3d(m_data_vet[m_data_tri[i].m_v[Z]].x,
                 m_data_vet[m_data_tri[i].m_v[Z]].y,
                 m_data_vet[m_data_tri[i].m_v[Z]].z);
    }
    glEnd();
}


// ------------------------------------------------------------------------------------------------
// ------------------------------------------------------------------------------------------------
void cBsp::m_draw_rec(  cNode *node, VVector &eye)
{
  if(!node)
    return;

  if(!node->back && !node->front)
  {
    // eh uma folha
    m_draw_node(node);
    return;
  }

  int t = node->tri;
  int s = calculate_side(m_data_nor[t], m_data_vet[m_data_tri[t].m_v[0]], eye);

  if(s == FRONT)
  {
    // estou na frente do no
    m_draw_rec(node->back, eye);
    m_draw_node(node);
    m_draw_rec(node->front, eye);
  }
  else if(s == BACK)
  {
    //estou atras do no
    m_draw_rec(node->front, eye);
    m_draw_node(node);
    m_draw_rec(node->back, eye);
  }
  else
  {
    m_draw_rec(node->front, eye);
    m_draw_rec(node->back, eye);
  }
}

// ------------------------------------------------------------------------------------------------
// ------------------------------------------------------------------------------------------------
void cBsp::calculate_bbox(void)
{
  m_bbox.max = m_data_vet[0];
  m_bbox.min = m_data_vet[0];

  for(int i = 1; i < (int)m_data_vet.size(); ++i)
  {
    m_bbox.max.x = max(m_bbox.max.x, m_data_vet[i].x);
    m_bbox.max.y = max(m_bbox.max.y, m_data_vet[i].y);
    m_bbox.max.z = max(m_bbox.max.z, m_data_vet[i].z);

    m_bbox.min.x = min(m_bbox.min.x, m_data_vet[i].x);
    m_bbox.min.y = min(m_bbox.min.y, m_data_vet[i].y);
    m_bbox.min.z = min(m_bbox.min.z, m_data_vet[i].z);
  }
}

// ------------------------------------------------------------------------------------------------
// ------------------------------------------------------------------------------------------------
int select_triangle(std::vector<cTriangle> &vet_tri, std::vector<VVector> &vet_vertex, std::vector<VVector> &vet_nor)
{
  int selected_triangle = 0;
  int size_cut = 0;
  int best_cuts = 100000;

  // TODO
 //selected_triangle = (int) vet_tri.size() / 2;

  // busca um triangulo que não corte nenhum outro
  // caso não encontre, escolhe o triangulo que corte menos
  for(int i = 0; i < (int)vet_tri.size(); ++i)
  {
    size_cut = 0;
    for(int j = 0; j < (int) vet_tri.size(); ++j)
    {
      if(i != j)
      {
        int s = calculate_side(vet_nor[i], vet_vertex[vet_tri[i].m_v[0]], vet_vertex[vet_tri[j].m_v[0]], vet_vertex[vet_tri[j].m_v[1]], vet_vertex[vet_tri[j].m_v[2]]);
        if(s == CUT)
          ++size_cut;
      }
    }
    if(size_cut == 0)
    {
      // nao teve nenhum corte
      selected_triangle = i;
      break;
    }
    else
    {
      // quarda o triangulo que teve menor quantidade de cortes
      if(size_cut < best_cuts)
      {
        size_cut  = best_cuts;
        selected_triangle = i;
      }
    }
  }

  return selected_triangle;
}

// ------------------------------------------------------------------------------------------------
// ------------------------------------------------------------------------------------------------
void cut_triangle(int selected_triangle, int other_triangle,
                          std::vector<cTriangle> &vet_tri,
                          std::vector<VVector> &vet_vertex,
                          std::vector<VVector> &vet_nor,
                          std::vector<cTriangle> &vet_tri_front,
                          std::vector<VVector> &vet_vertex_front,
                          std::vector<VVector> &vet_nor_front,
                          std::vector<cTriangle> &vet_tri_back,
                          std::vector<VVector> &vet_vertex_back,
                          std::vector<VVector> &vet_nor_back)
{
  // coordenadas do triangulo que vai ser cortado.
  VVector a = vet_vertex[vet_tri[other_triangle].m_v[0]];
  VVector b = vet_vertex[vet_tri[other_triangle].m_v[1]];
  VVector c = vet_vertex[vet_tri[other_triangle].m_v[2]];

  // coordenadas do plano (triangulo selecionado)
  VVector p0 = vet_vertex[vet_tri[selected_triangle].m_v[0]];
  VVector p1 = vet_vertex[vet_tri[selected_triangle].m_v[1]];
  VVector p2 = vet_vertex[vet_tri[selected_triangle].m_v[2]];

  VVector nor = vet_nor[selected_triangle];

  VVector ab, ac, bc;

  // verifica se corta o segmento AB
  bool b_ab = calculate_intersection(a, b, p0, nor, ab);

  // verifica se corta o segmento AC
  bool b_ac = calculate_intersection(a, c, p0, nor, ac);

  // verifica se corta o segmento BC
  bool b_bc = calculate_intersection(b, c, p0, nor, bc);

  int s;

  if(b_ab && !b_ac && !b_bc) // apenas o segmento ab
  {
    s =  calculate_side(nor, p0, ab, b, c);
    if(s == FRONT)
    {
      add_triangle(ab, b, c, vet_tri_front, vet_vertex_front, vet_nor_front);
      add_triangle(ab, c, a, vet_tri_back, vet_vertex_back, vet_nor_back);
    }
    else
    {
      add_triangle(ab, b, c, vet_tri_back, vet_vertex_back, vet_nor_back);
      add_triangle(ab, c, a, vet_tri_front, vet_vertex_front, vet_nor_front);
    }
  }


  else if(!b_ab && b_ac && !b_bc) // apenas o segmento ac
  {
    s =  calculate_side(nor, p0, ac, a , b);
    if(s == FRONT)
    {
      add_triangle(ac, a, b, vet_tri_front, vet_vertex_front, vet_nor_front);
      add_triangle(ac, b, c, vet_tri_back, vet_vertex_back, vet_nor_back);
    }
    else
    {
      add_triangle(ac, a, b, vet_tri_back, vet_vertex_back, vet_nor_back);
      add_triangle(ac, b, c, vet_tri_front, vet_vertex_front, vet_nor_front);
    }
  }


  else if(!b_ab && !b_ac && b_bc) // apenas o segmento bc
  {
    s =  calculate_side(nor, p0, bc, a , b);
    if(s == FRONT)
    {
      add_triangle(bc, a, b, vet_tri_front, vet_vertex_front, vet_nor_front);
      add_triangle(bc, c, a, vet_tri_back, vet_vertex_back, vet_nor_back);
    }
    else
    {
      add_triangle(bc, a, b, vet_tri_back, vet_vertex_back, vet_nor_back);
      add_triangle(bc, c, a, vet_tri_front, vet_vertex_front, vet_nor_front);
    }
  }


  else if(b_ab && b_ac && !b_bc) // cortou os segmentos ab e ac
  {
    s =  calculate_side(nor, p0, a, ab , ac);
    if(s == FRONT)
    {
      add_triangle(a, ab, ac, vet_tri_front, vet_vertex_front, vet_nor_front);

      add_triangle(b, ac,  ab, vet_tri_back, vet_vertex_back, vet_nor_back);
      add_triangle(b, c,  ac, vet_tri_back, vet_vertex_back, vet_nor_back);
    }
    else
    {
      add_triangle(a, ab, ac, vet_tri_back, vet_vertex_back, vet_nor_back);

      add_triangle(b, ac,  ab, vet_tri_front, vet_vertex_front, vet_nor_front);
      add_triangle(b, c,  ac, vet_tri_front, vet_vertex_front, vet_nor_front);
    }
  }


  else if(!b_ab && b_ac && b_bc ) // cortou os segmentos ac e bc
  {
    s =  calculate_side(nor, p0, c, ac, bc);
    if(s == FRONT)
    {
      add_triangle(c, ac, bc, vet_tri_front, vet_vertex_front, vet_nor_front);

      add_triangle(a, bc, ac, vet_tri_back, vet_vertex_back, vet_nor_back);
      add_triangle(a, b, bc, vet_tri_back, vet_vertex_back, vet_nor_back);
    }
    else
    {
      add_triangle(ac, bc, c, vet_tri_back, vet_vertex_back, vet_nor_back);

      add_triangle(a, bc, ac, vet_tri_front, vet_vertex_front, vet_nor_front);
      add_triangle(a, b, bc, vet_tri_front, vet_vertex_front, vet_nor_front);
    }
  }


  else if(b_ab && !b_ac && b_bc) // cortou os segmentos ab e bc
  {
    s =  calculate_side(nor, p0, b, bc, ab);
    if(s == FRONT)
    {
      add_triangle(b, bc, ab, vet_tri_front, vet_vertex_front, vet_nor_front);

      add_triangle(a, ab, bc, vet_tri_back, vet_vertex_back, vet_nor_back);
      add_triangle(a, bc, c, vet_tri_back, vet_vertex_back, vet_nor_back);
    }
    else
    {
      add_triangle(b, bc, ab, vet_tri_back, vet_vertex_back, vet_nor_back);

      add_triangle(a, ab, bc, vet_tri_front, vet_vertex_front, vet_nor_front);
      add_triangle(a, bc, c, vet_tri_front, vet_vertex_front, vet_nor_front);
    }
  }
  else
    return;
}

// ------------------------------------------------------------------------------------------------
// ------------------------------------------------------------------------------------------------
// http://en.wikipedia.org/wiki/Line-plane_intersection
// http://en.wikipedia.org/wiki/Matrix_inversion#Methods_of_matrix_inversion
// http://softsurfer.com/Archive/algorithm_0104/algorithm_0104B.htm#Line-Plane Intersection
bool calculate_intersection(VVector a, VVector b, VVector p0, VVector nor, VVector &res)
{
  double da = Dot(a - p0, nor);
  double db = Dot(b - p0, nor);

  if(da > -TOL && da < TOL) da = 0.0f;
  if(db > -TOL && db < TOL) db = 0.0f;

  if(da * db < -TOL)
  {
    double f = fabs(da) / (fabs(da) + fabs(db));
    res = a + ((b - a) * f);
    return true;
  }

  return false;
}

// ------------------------------------------------------------------------------------------------
// ------------------------------------------------------------------------------------------------
void add_triangle(VVector a, VVector b, VVector c,
                  std::vector<cTriangle> &vet_tri,
                  std::vector<VVector> &vet_ver,
                  std::vector<VVector> &vet_nor)
{
  int idx_a, idx_b, idx_c;
  std::vector<VVector>:: const_iterator it;

  // verifica se o ponto jah existe
  it = find(vet_ver.begin(), vet_ver.end(), a);
  if(it == vet_ver.end())
  {
    vet_ver.push_back(a);
    idx_a = (int) vet_ver.size() - 1;
  }
  else
    idx_a = it - vet_ver.begin();

  it = find(vet_ver.begin(), vet_ver.end(), b);
  if(it == vet_ver.end())
  {
    vet_ver.push_back(b);
    idx_b = (int) vet_ver.size() - 1;
  }
  else
    idx_b= it - vet_ver.begin();

  it = find(vet_ver.begin(), vet_ver.end(), c);
  if(it == vet_ver.end())
  {
    vet_ver.push_back(c);
    idx_c = (int) vet_ver.size() - 1;
  }
  else
    idx_c = it - vet_ver.begin();

  vet_tri.push_back(cTriangle(idx_a, idx_b, idx_c));

  // calcula a normal
  VVector nor = Cross(b - a, c - a);
  nor.Normalize();
  vet_nor.push_back(nor);
}

// ------------------------------------------------------------------------------------------------
// ------------------------------------------------------------------------------------------------
int calculate_side(VVector &normal_plane,  VVector &p,  VVector &a,  VVector &b,  VVector &c)
{
  double d0 = Dot(a - p, normal_plane);
  double d1 = Dot(b - p, normal_plane);
  double d2 = Dot(c - p, normal_plane);

  // tolerancia
  if(d0 > -TOL && d0 < TOL) d0 = 0.0f;
  if(d1 > -TOL && d1 < TOL) d1 = 0.0f;
  if(d2 > -TOL && d2 < TOL) d2 = 0.0f;

  if((d0 * d1 < -TOL) || (d0 * d2 < -TOL) || (d1 * d2 < -TOL))
    return CUT;

  if(d0 > TOL || d1 > TOL || d2 > TOL)
    return FRONT;
  else if(d0 < TOL || d1 < TOL || d2 < TOL)
    return BACK;

  // paralelo
  return BACK;
}

// ------------------------------------------------------------------------------------------------
// ------------------------------------------------------------------------------------------------
int calculate_side(VVector &normal_plane,  VVector &p,  VVector &a)
{
  double d = Dot(a - p, normal_plane);

  // tolerancia
  if(d > -TOL && d < TOL) d = 0.0f;

  if(d > TOL)
    return FRONT;
  else if(d < TOL)
    return BACK;

  return CUT;
}
